using System;
using System.Collections;
using System.Collections.Generic;
using static LightCAD.Three.MathUtils;

namespace LightCAD.Three
{
    public class Ray
    {
        #region scope properties or methods
        //private static Vector3 _vector = new Vector3();
        //private static Vector3 _segCenter = new Vector3();
        //private static Vector3 _segDir = new Vector3();
        //private static Vector3 _diff = new Vector3();
        //private static Vector3 _edge1 = new Vector3();
        //private static Vector3 _edge2 = new Vector3();
        //private static Vector3 _normal = new Vector3();
        private RayContext getContext()
        {
            return ThreadContext.getCurrThreadContext().RayCtx;
        }
        #endregion

        #region Properties

        public Vector3 origin;
        public Vector3 direction;

        #endregion

        #region constructor
        public Ray(Vector3 origin = null, Vector3 direction = null)
        {
            if (origin == null) origin = new Vector3();
            if (direction == null) direction = new Vector3(0, 0, -1);
            this.origin = origin;
            this.direction = direction;
        }
        #endregion

        #region methods
        public Ray set(Vector3 origin, Vector3 direction)
        {
            this.origin.copy(origin);
            this.direction.copy(direction);
            return this;
        }
        public Ray copy(Ray ray)
        {
            this.origin.copy(ray.origin);
            this.direction.copy(ray.direction);
            return this;
        }
        public Vector3 at(double t, Vector3 target = null)
        {
            target = target ?? new Vector3();
            return target.copy(this.origin).addScaledVector(this.direction, t);
            //return target.copy(this.direction).multiplyScalar(t).add(this.origin);
        }
        public Ray lookAt(Vector3 v)
        {
            this.direction.copy(v).sub(this.origin).normalize();
            return this;
        }
        public Ray recast(double t)
        {
            this.origin.copy(this.at(t, getContext()._vector));
            return this;
        }
        public Vector3 closestPointToPoint(Vector3 point, Vector3 target)
        {
            target.subVectors(point, this.origin);
            var directionDistance = target.dot(this.direction);
            if (directionDistance < 0)
            {
                return target.copy(this.origin);
            }
            return target.copy(this.origin).addScaledVector(this.direction, directionDistance);
            //return target.copy(this.direction).multiplyScalar(directionDistance).add(this.origin);
        }
        public double distanceToPoint(Vector3 point)
        {
            return JMath.sqrt(this.distanceSqToPoint(point));
        }
        public double distanceSqToPoint(Vector3 point)
        {
            var _vector = getContext()._vector;
            var directionDistance = _vector.subVectors(point, this.origin).dot(this.direction);
            // point behind the ray
            if (directionDistance < 0)
            {
                return this.origin.distanceToSquared(point);
            }
            _vector.copy(this.origin).addScaledVector(this.direction, directionDistance);
            // _vector.copy(this.direction).multiplyScalar(directionDistance).add(this.origin);
            return _vector.distanceToSquared(point);
        }
        public double distanceSqToSegment(Vector3 v0, Vector3 v1, Vector3 optionalPointOnRay, Vector3 optionalPointOnSegment)
        {
            // from https://github.com/pmjoniak/GeometricTools/blob/master/GTEngine/Include/Mathematics/GteDistRaySegment.h
            // It returns the min distance between the ray and the segment
            // defined by v0 and v1
            // It can also set two optional targets :
            // - The closest point on the ray
            // - The closest point on the segment
            var ctx = getContext();
            var _segCenter = ctx._segCenter;
            var _segDir = ctx._segDir;
            var _diff = ctx._diff;

            _segCenter.copy(v0).add(v1).multiplyScalar(0.5);
            _segDir.copy(v1).sub(v0).normalize();
            _diff.copy(this.origin).sub(_segCenter);
            var segExtent = v0.distanceTo(v1) * 0.5;
            var a01 = -this.direction.dot(_segDir);
            var b0 = _diff.dot(this.direction);
            var b1 = -_diff.dot(_segDir);
            var c = _diff.lengthSq();
            var det = JMath.abs(1 - a01 * a01);
            double s0, s1, sqrDist, extDet;
            if (det > 0)
            {
                // The ray and segment are not parallel.
                s0 = a01 * b1 - b0;
                s1 = a01 * b0 - b1;
                extDet = segExtent * det;
                if (s0 >= 0)
                {
                    if (s1 >= -extDet)
                    {
                        if (s1 <= extDet)
                        {
                            // region 0
                            // Minimum at interior points of ray and segment.
                            var invDet = 1 / det;
                            s0 *= invDet;
                            s1 *= invDet;
                            sqrDist = s0 * (s0 + a01 * s1 + 2 * b0) + s1 * (a01 * s0 + s1 + 2 * b1) + c;
                        }
                        else
                        {
                            // region 1
                            s1 = segExtent;
                            s0 = JMath.max(0, -(a01 * s1 + b0));
                            sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
                        }
                    }
                    else
                    {
                        // region 5
                        s1 = -segExtent;
                        s0 = JMath.max(0, -(a01 * s1 + b0));
                        sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
                    }
                }
                else
                {
                    if (s1 <= -extDet)
                    {
                        // region 4
                        s0 = JMath.max(0, -(-a01 * segExtent + b0));
                        s1 = (s0 > 0) ? -segExtent : JMath.min(JMath.max(-segExtent, -b1), segExtent);
                        sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
                    }
                    else if (s1 <= extDet)
                    {
                        // region 3
                        s0 = 0;
                        s1 = JMath.min(JMath.max(-segExtent, -b1), segExtent);
                        sqrDist = s1 * (s1 + 2 * b1) + c;
                    }
                    else
                    {
                        // region 2
                        s0 = JMath.max(0, -(a01 * segExtent + b0));
                        s1 = (s0 > 0) ? segExtent : JMath.min(JMath.max(-segExtent, -b1), segExtent);
                        sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
                    }
                }
            }
            else
            {
                // Ray and segment are parallel.
                s1 = (a01 > 0) ? -segExtent : segExtent;
                s0 = JMath.max(0, -(a01 * s1 + b0));
                sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
            }
            if (optionalPointOnRay != null)
            {
                optionalPointOnRay.copy(this.origin).addScaledVector(this.direction, s0);
                // optionalPointOnRay.copy(this.direction).multiplyScalar(s0).add(this.origin);
            }
            if (optionalPointOnSegment != null)
            {
                optionalPointOnSegment.copy(_segCenter).addScaledVector(_segDir, s1);
                //optionalPointOnSegment.copy(_segDir).multiplyScalar(s1).add(_segCenter);
            }
            return sqrDist;
        }
        public Vector3 intersectSphere(Sphere sphere, Vector3 target)
        {
            var ctx = getContext();
            var _vector = ctx._vector;
            _vector.subVectors(sphere.center, this.origin);
            var tca = _vector.dot(this.direction);
            var d2 = _vector.dot(_vector) - tca * tca;
            var radius2 = sphere.radius * sphere.radius;
            if (d2 > radius2) return null;
            var thc = JMath.sqrt(radius2 - d2);
            // t0 = first intersect point - entrance on front of sphere
            var t0 = tca - thc;
            // t1 = second intersect point - exit point on back of sphere
            var t1 = tca + thc;

            // test to see if t1 is behind the ray - if so, return null
            if (t1 < 0) return null;
            // test to see if t0 is behind the ray:
            // if it is, the ray is inside the sphere, so return the second exit point scaled by t1,
            // in order to always return an intersect point that is in front of the ray.
            if (t0 < 0) return this.at(t1, target);
            // else t0 is in front of the ray, so return the first collision point scaled by t0
            return this.at(t0, target);
        }
        public bool intersectsSphere(Sphere sphere)
        {
            return this.distanceSqToPoint(sphere.center) <= (sphere.radius * sphere.radius);
        }
        public double distanceToPlane(Plane plane)
        {
            var denominator = plane.normal.dot(this.direction);
            if (denominator == 0)
            {
                // line is coplanar, return origin
                if (plane.distanceToPoint(this.origin) == 0)
                {
                    return 0;
                }
                // Null is preferable to undefined since undefined means.... it is undefined
                return double.NaN;
            }
            var t = -(this.origin.dot(plane.normal) + plane.constant) / denominator;
            // Return if the ray never intersects the plane
            return t >= 0 ? t : double.NaN;
        }
        public Vector3 intersectPlane(Plane plane, Vector3 target = null)
        {
            var t = this.distanceToPlane(plane);
            if (double.IsNaN(t))
            {
                return null;
            }
            return this.at(t, target);
        }
        public bool intersectsPlane(Plane plane)
        {
            // check if the ray lies on the plane first
            var distToPoint = plane.distanceToPoint(this.origin);
            if (distToPoint == 0)
            {
                return true;
            }
            var denominator = plane.normal.dot(this.direction);
            if (denominator * distToPoint < 0)
            {
                return true;
            }
            // ray origin is behind the plane (and is pointing behind it)
            return false;
        }
        public Vector3 intersectBox(Box3 box, Vector3 target = null)
        {
            target = target ?? new Vector3();
            double tmin, tmax, tymin, tymax, tzmin, tzmax;
            double invdirx = 1 / this.direction.x,
                invdiry = 1 / this.direction.y,
                invdirz = 1 / this.direction.z;
            var origin = this.origin;
            if (invdirx >= 0)
            {
                tmin = (box.min.x - origin.x) * invdirx;
                tmax = (box.max.x - origin.x) * invdirx;
            }
            else
            {
                tmin = (box.max.x - origin.x) * invdirx;
                tmax = (box.min.x - origin.x) * invdirx;
            }
            if (invdiry >= 0)
            {
                tymin = (box.min.y - origin.y) * invdiry;
                tymax = (box.max.y - origin.y) * invdiry;
            }
            else
            {
                tymin = (box.max.y - origin.y) * invdiry;
                tymax = (box.min.y - origin.y) * invdiry;
            }
            if ((tmin > tymax) || (tymin > tmax)) return null;
            if (tymin > tmin || isNaN(tmin)) tmin = tymin;
            if (tymax < tmax || isNaN(tmax)) tmax = tymax;
            if (invdirz >= 0)
            {
                tzmin = (box.min.z - origin.z) * invdirz;
                tzmax = (box.max.z - origin.z) * invdirz;
            }
            else
            {
                tzmin = (box.max.z - origin.z) * invdirz;
                tzmax = (box.min.z - origin.z) * invdirz;
            }
            if ((tmin > tzmax) || (tzmin > tmax)) return null;
            if (tzmin > tmin || isNaN(tmin)) tmin = tzmin;
            if (tzmax < tmax || isNaN(tmax)) tmax = tzmax;
            //return point closest to the ray (positive side)
            if (tmax < 0) return null;
            return this.at(tmin >= 0 ? tmin : tmax, target);
        }
        public bool intersectsBox(Box3 box)
        {
            return this.intersectBox(box, getContext()._vector) != null;
        }
        public Vector3 intersectTriangle(Vector3 a, Vector3 b, Vector3 c, bool backfaceCulling, Vector3 target = null)
        {
            // Compute the offset origin, edges, and normal.
            // from https://github.com/pmjoniak/GeometricTools/blob/master/GTEngine/Include/Mathematics/GteIntrRay3Triangle3.h
            var ctx = getContext();
            var _edge1 = ctx._edge1;
            var _edge2 = ctx._edge2;
            var _normal = ctx._normal;
            var _diff = ctx._diff;

            _edge1.subVectors(b, a);
            _edge2.subVectors(c, a);
            _normal.crossVectors(_edge1, _edge2);
            // Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction,
            // E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by
            //   |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
            //   |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
            //   |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)
            var DdN = this.direction.dot(_normal);
            int sign;
            if (DdN > 0)
            {
                if (backfaceCulling) return null;
                sign = 1;
            }
            else if (DdN < 0)
            {
                sign = -1;
                DdN = -DdN;
            }
            else
            {
                return null;
            }
            _diff.subVectors(this.origin, a);
            var DdQxE2 = sign * this.direction.dot(_edge2.crossVectors(_diff, _edge2));
            // b1 < 0, no intersection
            if (DdQxE2 < 0)
            {
                return null;
            }
            var DdE1xQ = sign * this.direction.dot(_edge1.cross(_diff));
            // b2 < 0, no intersection
            if (DdE1xQ < 0)
            {
                return null;
            }
            // b1+b2 > 1, no intersection
            if (DdQxE2 + DdE1xQ > DdN)
            {
                return null;
            }
            // Line intersects triangle, check if ray does.
            var QdN = -sign * _diff.dot(_normal);
            // t < 0, no intersection
            if (QdN < 0)
            {
                return null;
            }
            // Ray intersects triangle.
            return this.at(QdN / DdN, target);
        }
        public Ray applyMatrix4(Matrix4 matrix4)
        {
            this.origin.applyMatrix4(matrix4);
            this.direction.transformDirection(matrix4);
            return this;
        }
        public bool equals(Ray ray)
        {
            return ray.origin.equals(this.origin) && ray.direction.equals(this.direction);
        }
        public Ray clone()
        {
            return new Ray().copy(this);
        }
        #endregion

    }
}
